import numpy as np
import cvxopt

from ..robot_state import RobotState, LegState, MotionState
from ..utils import PriorityTasks
from ..robotics import Robotics
from ..utils import Transformation


class WBC:
    def __init__(self, model_nv: int, QP_nv: int, QP_nc: int, miu: float, dt: float) -> None:
        super().__init__()
        self.QP_nv = QP_nv
        self.QP_nc = QP_nc

        self.model_nv = model_nv
        self.tau_upp_stand_L = np.array([10, 20, 40])
        self.tau_low_stand_L = np.array([-10, -20, -40])
        self.tau_upp_walk_L = np.array([25, 40, 40])
        self.tau_low_walk_L = np.array([-25, -40, -40])

        self.f_z_low = 10.0
        self.f_z_upp = 1400.0
        self.legStateCur = LegState.LST
        self.motionStateCur = MotionState.STAND
        self.miu = miu

        self.dyn_M = np.zeros((self.model_nv, self.model_nv))
        self.dyn_M_inv = np.zeros((self.model_nv, self.model_nv))
        self.dyn_Ag = np.zeros((6, self.model_nv))
        self.dyn_dAg = np.zeros((6, self.model_nv))
        self.dyn_Non = np.zeros(self.model_nv)
        self.Jc = np.zeros((6, self.model_nv))
        self.dJc = np.zeros((6, self.model_nv))
        self.Jfe = np.zeros((6, self.model_nv))
        self.dJfe = np.zeros((6, self.model_nv))
        self.Jfe_L = np.zeros((6, self.model_nv))
        self.Jfe_R = np.zeros((6, self.model_nv))
        self.J_hd_l = np.zeros((6, self.model_nv))
        self.J_hd_r = np.zeros((6, self.model_nv))
        self.dJ_hd_l = np.zeros((6, self.model_nv))
        self.dJ_hd_r = np.zeros((6, self.model_nv))
        self.Jsw = np.zeros((6, self.model_nv))
        self.dJsw = np.zeros((6, self.model_nv))
        self.fe_rot_sw_W = np.zeros((3, 3))
        self.fe_pos_sw_W = np.zeros(3)
        self.hd_l_pos_cur_W = np.zeros(3)
        self.hd_r_pos_cur_W = np.zeros(3)
        self.hd_l_rot_cur_W = np.zeros((3, 3))
        self.hd_r_rot_cur_W = np.zeros((3, 3))
        self.fe_l_pos_des_W = np.zeros(3)
        self.fe_r_pos_des_W = np.zeros(3)
        self.fe_l_rot_des_W = np.zeros((3, 3))
        self.fe_r_rot_des_W = np.zeros((3, 3))
        self.fe_l_pos_cur_W = np.zeros(3)
        self.fe_r_pos_cur_W = np.zeros(3)
        self.fe_l_rot_cur_W = np.zeros((3, 3))
        self.fe_r_rot_cur_W = np.zeros((3, 3))
        self.fe_rot = np.eye(3)
        self.q = np.zeros(self.model_nv + 1)
        self.dq = np.zeros(self.model_nv)
        self.ddq = np.zeros(self.model_nv)
        self.Fr_ff = np.zeros(12)
        self.delta_ddq = np.zeros(self.model_nv)
        self.delta_Fr = np.zeros(12)

        self.x_opt = np.zeros(self.QP_nv)
        self.ddq_opt = np.zeros(self.model_nv)
        self.fr_opt = np.zeros(12)
        self.tau_opt = np.zeros(self.model_nv - 6)

        self.Q1 = np.zeros((self.model_nv, self.model_nv))
        self.Q2 = np.zeros((self.model_nv, self.model_nv))

        self.delta_q_final_kin = np.zeros(self.model_nv)
        self.dq_final_kin = np.zeros(self.model_nv)
        self.ddq_final_kin = np.zeros(self.model_nv)

        self.tauJointRes = np.zeros(self.model_nv)
        self.fe_L_rot_L_off = np.zeros((3, 3))
        self.fe_R_rot_L_off = np.zeros((3, 3))
        self.l_shoulder_pitch = 0.0
        self.r_shoulder_pitch = 0.0
        self.pCoMDes = np.zeros(3)
        self.pCoMCur = np.zeros(3)

        self.kin_tasks_walk = PriorityTasks(self.model_nv)
        self.kin_tasks_stand = PriorityTasks(self.model_nv)

        self.timeStep = dt
        self.Sf = np.zeros((6, self.model_nv))
        self.Sf[0: 6, 0: 6] = np.eye(6)
        self.St_qpV2 = np.zeros((self.model_nv, self.model_nv - 6))
        self.St_qpV2[6: self.model_nv, 0: self.model_nv - 6] = np.eye(self.model_nv - 6)
        self.St_qpV1 = np.zeros((self.model_nv, 6))
        self.St_qpV1[0: 6, 0: 6] = np.eye(6)

        self.nWSR = 100
        self.last_nWSR = 0
        self.cpu_time = 0.1
        self.last_cpu_time = 0

        self.qpStatus = 0

        self.J_base = np.zeros((6, self.model_nv))
        self.dJ_base = np.zeros((6, self.model_nv))
        self.Jcom = np.zeros((3, self.model_nv))
        self.J_hip_link = np.zeros((6, self.model_nv))
        self.base_pos_des = np.zeros(3)
        self.base_pos = np.zeros(3)
        self.base_rpy_des = np.zeros(3)
        self.base_rpy_cur = np.zeros(3)
        self.hip_link_pos = np.zeros(3)
        self.hip_link_rot = np.zeros((3, 3))
        self.base_rot = np.zeros((3, 3))
        self.swing_fe_pos_des_W = np.zeros(3)
        self.swing_fe_rpy_des_W = np.zeros(3)
        self.stance_fe_pos_cur_W = np.zeros(3)
        self.stance_fe_rot_cur_W = np.zeros((3, 3))
        self.stanceDesPos_W = np.zeros(3)
        self.des_ddq = np.zeros(self.model_nv)
        self.des_dq = np.zeros(self.model_nv)
        self.des_delta_q = np.zeros(self.model_nv)
        self.des_q = np.zeros(self.model_nv)
        self.qIniDes = np.zeros(self.model_nv + 1)
        self.qIniCur = np.zeros(self.model_nv + 1)
        self.foot_height = 0.0

        self.QP_nv_des = 19
        self.QP_nc_des = 22

        self.qp_H = cvxopt.matrix()
        self.qp_A = cvxopt.matrix()
        self.qp_g = cvxopt.matrix()
        self.qp_lbA = cvxopt.matrix()
        self.qp_ubA = cvxopt.matrix()
        self.xOpt_iniGuess = cvxopt.matrix()

        self.kin_tasks_walk.addTask("static_Contact")
        self.kin_tasks_walk.addTask("Roll_Pitch_Yaw_Pz")
        self.kin_tasks_walk.addTask("PxPy")
        self.kin_tasks_walk.addTask("SwingLeg")
        self.kin_tasks_walk.addTask("PosRot")

        taskOrder_walk: list[str] = []
        taskOrder_walk.append("static_Contact")
        taskOrder_walk.append("PosRot")
        taskOrder_walk.append("SwingLeg")

        self.kin_tasks_walk.build_priority(taskOrder_walk)

        self.kin_tasks_stand.addTask("static_Contact")
        self.kin_tasks_stand.addTask("CoMTrack")
        self.kin_tasks_stand.addTask("HipRPY")
        self.kin_tasks_stand.addTask("Pz")
        self.kin_tasks_stand.addTask("CoMXY_HipRPY")
        self.kin_tasks_stand.addTask("Roll_Pitch_Yaw")

        taskOrder_stand: list[str] = []
        taskOrder_stand.append("static_Contact")
        taskOrder_stand.append("CoMXY_HipRPY")
        taskOrder_stand.append("Pz")

        self.kin_tasks_stand.build_priority(taskOrder_stand)

    def setQini(self, qIniDes: np.ndarray, qIniCur: np.ndarray):
        self.qIniDes[:] = qIniDes
        self.qIniCur[:] = qIniCur

    def calculate(self):

        qp_A1 = np.zeros((6, self.QP_nv))
        qp_A1[0: 6, 0: 6] = self.Sf @ self.dyn_M @ self.St_qpV1
        qp_A1[0: 6, 6: 18] = -self.Sf @ self.Jfe.T
        eqRes = -self.Sf @ self.dyn_M @ self.ddq_final_kin - self.Sf @ self.dyn_Non + self.Sf @ self.Jfe.T @ self.Fr_ff

        Rfe = np.zeros((3, 3))

        if self.motionStateCur == MotionState.STAND:
            Rfe[:, :] = self.fe_l_rot_cur_W @ self.fe_rot.T
        else:
            Rfe[:, :] = self.stance_fe_rot_cur_W @ self.fe_rot.T

        Mw2b = np.zeros((12, 12))
        Mw2b[0: 3, 0: 3] = Rfe.T
        Mw2b[3: 6, 3: 6] = Rfe.T
        Mw2b[6: 9, 6: 9] = Rfe.T
        Mw2b[9: 12, 9: 12] = Rfe.T

        W = np.zeros((16, 12))
        W[0, 0] = 1
        W[0, 2] = np.sqrt(2) / 2.0 * self.miu
        W[1, 0] = -1
        W[1, 2] = np.sqrt(2) / 2.0 * self.miu
        W[2, 1] = 1
        W[2, 2] = np.sqrt(2) / 2.0 * self.miu
        W[3, 1] = -1
        W[3, 2] = np.sqrt(2) / 2.0 * self.miu
        W[4: 8, 2: 6] = np.eye(4)
        W[8: 16, 6: 12] = W[0: 8, 0: 6]
        W = W @ Mw2b

        f_low = np.zeros(16)
        f_upp = np.zeros(16)
        tau_upp_fe = np.zeros(3)
        tau_low_fe = np.zeros(3)
        if self.motionStateCur == MotionState.STAND:
            tau_upp_fe[:] = self.tau_upp_stand_L
            tau_low_fe[:] = self.tau_low_stand_L
        else:
            tau_upp_fe[:] = self.tau_upp_walk_L
            tau_low_fe[:] = self.tau_low_walk_L

        f_upp[0: 8] = [1e10, 1e10, 1e10, 1e10, self.f_z_upp, tau_upp_fe[0], tau_upp_fe[1], tau_upp_fe[2]]
        f_upp[8: 16] = f_upp[0: 8]
        f_low[0: 8] = [0.0, 0.0, 0.0, 0.0, self.f_z_low, tau_low_fe[0], tau_low_fe[1], tau_low_fe[2]]
        f_low[8: 16] = f_low[0: 8]

        if self.motionStateCur == MotionState.WALK or self.motionStateCur == MotionState.WALK2STAND:
            if self.legStateCur == LegState.LST:
                f_upp[12: 16] = 0
                f_low[12: 16] = 0
                f_low[8: 12] = -1e-7

            elif self.legStateCur == LegState.RST:
                f_upp[4: 8] = 0
                f_low[4: 8] = 0
                f_low[0: 4] = -1e-7

        qp_A2 = np.zeros((16, 18))
        qp_A2[0: 16, 6: 18] = W
        neqRes_low = np.zeros(16)
        neqRes_upp = np.zeros(16)

        neqRes_low[:] = f_low - W @ self.Fr_ff
        neqRes_upp[:] = f_upp - W @ self.Fr_ff

        qp_A_final = np.zeros((self.QP_nc, self.QP_nv))
        qp_A_final[0: 6, 0: 18] = qp_A1
        qp_A_final[6: 22, 0: 18] = qp_A2

        qp_lbA = np.zeros(22)
        qp_ubA = np.zeros(22)

        qp_lbA[0: 6] = eqRes
        qp_lbA[6: 22] = neqRes_low
        qp_ubA[0: 6] = eqRes
        qp_ubA[6: 22] = neqRes_upp

        eigen_qp_H = np.zeros((self.QP_nv, self.QP_nv))
        Q2 = np.eye(6)
        Q1 = np.eye(12)
        eigen_qp_H[0: 6, 0: 6] = Q2 * 2.0 * 1e3
        eigen_qp_H[6: 18, 6: 18] = Q1 * 2.0 * 1e1

        self.qp_H = cvxopt.matrix(eigen_qp_H)
        self.qp_A = cvxopt.matrix(np.vstack((qp_A_final, -qp_A_final)))
        self.qp_lbA = cvxopt.matrix(qp_lbA)
        self.qp_ubA = cvxopt.matrix(qp_ubA)
        b = cvxopt.matrix(np.hstack((qp_ubA, -qp_lbA)))
        self.qp_g = cvxopt.matrix(np.zeros(self.QP_nv))

        self.nWSR = 200
        self.cpu_time = self.timeStep
        res = cvxopt.solvers.qp(self.qp_H, self.qp_g, self.qp_A, b)

        x_opt = res['x']
        for i in range(self.QP_nv):
            self.x_opt[i] = x_opt[i]

        self.ddq_opt[:] = self.ddq_final_kin
        self.ddq_opt[0: 6] += self.x_opt[0: 6]
        self.fr_opt = self.Fr_ff + self.x_opt[6: 18]

        tauRes = (self.dyn_M @ self.ddq_opt + self.dyn_Non) - self.Jfe.T @ self.fr_opt

        self.tauJointRes = tauRes[6: self.model_nv]

        self.last_nWSR = self.nWSR
        self.last_cpu_time = self.cpu_time

    def read_state(self, robot_state: RobotState):
        self.fe_L_rot_L_off[:, :] = robot_state.fe_L_rot_L_off
        self.fe_R_rot_L_off[:, :] = robot_state.fe_R_rot_L_off

        self.base_rpy_des[:] = robot_state.base_rpy_des
        self.base_rpy_cur[:] = robot_state.rpy
        self.base_pos_des[:] = robot_state.base_pos_des
        self.swing_fe_pos_des_W[:] = robot_state.swing_fe_pos_des_W
        self.swing_fe_rpy_des_W[:] = robot_state.swing_fe_rpy_des_W
        self.stance_fe_pos_cur_W[:] = robot_state.stance_fe_pos_cur_W
        self.stance_fe_rot_cur_W[:, :] = robot_state.stance_fe_rot_cur_W
        self.stanceDesPos_W[:] = robot_state.stanceDesPos_W
        self.fe_l_pos_cur_W[:] = robot_state.fe_l_pos_W
        self.fe_r_pos_cur_W[:] = robot_state.fe_r_pos_W
        self.fe_l_rot_cur_W[:, :] = robot_state.fe_l_rot_W
        self.fe_r_rot_cur_W[:, :] = robot_state.fe_r_rot_W
        self.fe_rot[:, :] = robot_state.fe_rot
        self.des_ddq[:] = robot_state.des_ddq
        self.des_dq[:] = robot_state.des_dq
        self.des_delta_q[:] = robot_state.des_delta_q
        self.des_q[:] = robot_state.des_q

        self.J_base[:, :] = robot_state.J_base
        self.dJ_base[:, :] = robot_state.dJ_base
        self.base_rot[:, :] = robot_state.base_rot
        self.base_pos[:] = robot_state.base_pos
        self.hip_link_pos[:] = robot_state.hip_link_pos
        self.hip_link_rot[:, :] = robot_state.hip_link_rot
        self.J_hip_link[:, :] = robot_state.J_hip_link

        self.Jfe = np.zeros((12, self.model_nv))
        self.Jfe[0: 6, 0: self.model_nv] = robot_state.J_l
        self.Jfe[6: 12, 0: self.model_nv] = robot_state.J_r
        self.dJfe = np.zeros((12, self.model_nv))
        self.dJfe[0: 6, 0: self.model_nv] = robot_state.dJ_l
        self.dJfe[6: 12, 0: self.model_nv] = robot_state.dJ_r
        self.Fr_ff[:] = robot_state.Fr_ff
        self.dyn_M[:, :] = robot_state.dyn_M
        self.dyn_M_inv[:, :] = robot_state.dyn_M_inv
        self.dyn_Ag[:, :] = robot_state.dyn_Ag
        self.dyn_dAg[:, :] = robot_state.dyn_dAg
        self.dyn_Non[:] = robot_state.dyn_Non
        self.dq[:] = robot_state.dq
        self.q[:] = robot_state.q
        self.legStateCur = robot_state.legState
        self.motionStateCur = robot_state.motionState

        if self.legStateCur == LegState.LST:
            self.Jc[:, :] = robot_state.J_l
            self.dJc[:, :] = robot_state.dJ_l
            self.Jsw[:, :] = robot_state.J_r
            self.dJsw[:, :] = robot_state.dJ_r
            self.fe_pos_sw_W[:] = robot_state.fe_r_pos_W
            self.fe_rot_sw_W[:, :] = robot_state.fe_r_rot_W
        else:
            self.Jc[:, :] = robot_state.J_r
            self.dJc[:, :] = robot_state.dJ_r
            self.Jsw[:, :] = robot_state.J_l
            self.dJsw[:, :] = robot_state.dJ_l
            self.fe_pos_sw_W[:] = robot_state.fe_l_pos_W
            self.fe_rot_sw_W[:, :] = robot_state.fe_l_rot_W

        self.Jcom[:, :] = robot_state.Jcom_W
        self.pCoMCur[:] = robot_state.pCoM_W

        self.stance_fe_pos_cur_W = robot_state.stance_fe_pos_cur_W
        self.foot_height = robot_state.foot_height

    def write_state(self, robot_state: RobotState):
        robot_state.wbc_ddq_final[:] = self.ddq_opt
        robot_state.wbc_tauJointRes[:] = self.tauJointRes
        robot_state.wbc_FrRes[:] = self.fr_opt
        robot_state.qp_cpuTime = self.cpu_time
        robot_state.qp_nWSR = self.nWSR
        robot_state.qp_status = self.qpStatus

        robot_state.wbc_delta_q_final[:] = self.delta_q_final_kin
        robot_state.wbc_dq_final[:] = self.dq_final_kin
        robot_state.wbc_ddq_final[:] = self.ddq_final_kin

    def handle_task(self, robotics: Robotics):
        id = self.kin_tasks_walk.getId("static_Contact")
        self.kin_tasks_walk.task_list[id].errX = np.zeros(3)
        self.kin_tasks_walk.task_list[id].derrX = np.zeros(3)
        self.kin_tasks_walk.task_list[id].ddxDes = np.zeros(3)
        self.kin_tasks_walk.task_list[id].dxDes = np.zeros(3)
        self.kin_tasks_walk.task_list[id].kp = np.zeros((3, 3))
        self.kin_tasks_walk.task_list[id].kd = np.zeros((3, 3))
        self.kin_tasks_walk.task_list[id].J = self.Jc[0: 3, 0: self.model_nv].copy()
        self.kin_tasks_walk.task_list[id].dJ = self.dJc[0: 3, 0: self.model_nv].copy()
        self.kin_tasks_walk.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_walk.getId("Roll_Pitch_Yaw_Pz")
        self.kin_tasks_walk.task_list[id].errX = np.zeros(4)
        desRot = Transformation.eul2Rot(self.base_rpy_des[0], self.base_rpy_des[1], self.base_rpy_des[2])
        self.kin_tasks_walk.task_list[id].errX[:3] = Transformation.diffRot(self.base_rot, desRot)
        self.kin_tasks_walk.task_list[id].errX[3] = self.base_pos_des[2] - self.q[2]
        self.kin_tasks_walk.task_list[id].derrX = np.zeros(4)
        self.kin_tasks_walk.task_list[id].derrX[:3] = -self.dq[3: 6]
        self.kin_tasks_walk.task_list[id].derrX[3] = -self.q[2]
        self.kin_tasks_walk.task_list[id].ddxDes = np.zeros(4)
        self.kin_tasks_walk.task_list[id].dxDes = np.zeros(4)
        self.kin_tasks_walk.task_list[id].kp = np.eye(4) * 2000
        self.kin_tasks_walk.task_list[id].kd = np.eye(4) * 100
        taskMap = np.zeros((4, 6))
        taskMap[0, 3] = 1
        taskMap[1, 4] = 1
        taskMap[2, 5] = 1
        taskMap[3, 2] = 1
        self.kin_tasks_walk.task_list[id].J = taskMap @ self.J_base
        self.kin_tasks_walk.task_list[id].dJ = taskMap @ self.dJ_base
        self.kin_tasks_walk.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_walk.getId("PxPy")
        self.kin_tasks_walk.task_list[id].errX = np.zeros(2)
        self.kin_tasks_walk.task_list[id].errX[: 2] = self.des_dq[0: 2] * self.timeStep
        self.kin_tasks_walk.task_list[id].derrX = np.zeros(2)
        self.kin_tasks_walk.task_list[id].ddxDes = np.zeros(2)
        self.kin_tasks_walk.task_list[id].dxDes = np.zeros(2)
        self.kin_tasks_walk.task_list[id].kp = np.eye(2) * 500
        self.kin_tasks_walk.task_list[id].kd = np.eye(2) * 50
        taskMap = np.zeros((2, 6))
        taskMap[0, 0] = 1
        taskMap[1, 1] = 1
        self.kin_tasks_walk.task_list[id].J = taskMap @ self.J_base
        self.kin_tasks_walk.task_list[id].dJ = taskMap @ self.dJ_base
        self.kin_tasks_walk.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_walk.getId("PosRot")
        self.kin_tasks_walk.task_list[id].errX = np.zeros(6)
        self.kin_tasks_walk.task_list[id].errX[: 3] = self.base_pos_des - self.q[: 3]
        self.kin_tasks_walk.task_list[id].errX[2] += self.stance_fe_pos_cur_W[2] - self.foot_height
        if np.fabs(self.kin_tasks_walk.task_list[id].errX[0]) >= 0.02:
            self.kin_tasks_walk.task_list[id].errX[0] = 0.02 * np.sign(self.kin_tasks_walk.task_list[id].errX[0])
        if np.fabs(self.kin_tasks_walk.task_list[id].errX[1]) >= 0.01:
            self.kin_tasks_walk.task_list[id].errX[1] = 0.01 * np.sign(self.kin_tasks_walk.task_list[id].errX[1])
        desRot = Transformation.eul2Rot(self.base_rpy_des[0], self.base_rpy_des[1], self.base_rpy_des[2])
        self.kin_tasks_walk.task_list[id].errX[3: 6] = Transformation.diffRot(self.base_rot, desRot)
        self.kin_tasks_walk.task_list[id].derrX = np.zeros(6)
        self.kin_tasks_walk.task_list[id].ddxDes = np.zeros(6)
        self.kin_tasks_walk.task_list[id].dxDes = np.zeros(6)
        self.kin_tasks_walk.task_list[id].kp = np.eye(6) * 10
        self.kin_tasks_walk.task_list[id].kp[3: 6, 3: 6] = np.eye(3) * 2000
        self.kin_tasks_walk.task_list[id].kd = np.eye(6) * 2
        self.kin_tasks_walk.task_list[id].kd[3: 6, 3: 6] = np.eye(3) * 100
        self.kin_tasks_walk.task_list[id].J = self.J_base.copy()
        self.kin_tasks_walk.task_list[id].dJ = self.dJ_base.copy()
        self.kin_tasks_walk.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_walk.getId("SwingLeg")
        self.kin_tasks_walk.task_list[id].errX = np.zeros(6)
        self.kin_tasks_walk.task_list[id].errX[: 3] = self.swing_fe_pos_des_W - self.fe_pos_sw_W
        desRot = Transformation.eul2Rot(self.swing_fe_rpy_des_W[0], self.swing_fe_rpy_des_W[1],
                                        self.swing_fe_rpy_des_W[2])
        self.kin_tasks_walk.task_list[id].errX[3: 6] = Transformation.diffRot(self.fe_rot_sw_W, desRot @ self.fe_rot)
        self.kin_tasks_walk.task_list[id].derrX = np.zeros(6)
        self.kin_tasks_walk.task_list[id].ddxDes = np.zeros(6)
        self.kin_tasks_walk.task_list[id].dxDes = np.zeros(6)
        self.kin_tasks_walk.task_list[id].kp = np.eye(6) * 2000
        self.kin_tasks_walk.task_list[id].kp[2, 2] = self.kin_tasks_walk.task_list[id].kp[2, 2] * 0.1
        self.kin_tasks_walk.task_list[id].kd = np.eye(6) * 20
        self.kin_tasks_walk.task_list[id].J = self.Jsw.copy()
        self.kin_tasks_walk.task_list[id].dJ = self.dJsw.copy()
        self.kin_tasks_walk.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_stand.getId("static_Contact")
        self.kin_tasks_stand.task_list[id].errX = np.zeros(12)
        self.kin_tasks_stand.task_list[id].derrX = np.zeros(12)
        self.kin_tasks_stand.task_list[id].ddxDes = np.zeros(12)
        self.kin_tasks_stand.task_list[id].dxDes = np.zeros(12)
        self.kin_tasks_stand.task_list[id].kp = np.eye(12) * 0
        self.kin_tasks_stand.task_list[id].kd = np.eye(12) * 0
        self.kin_tasks_stand.task_list[id].J = np.zeros((12, self.model_nv))
        taskCtMap = np.zeros((3, 3))
        taskCtMap[0, 0] = 0
        taskCtMap[1, 1] = 1
        taskCtMap[2, 2] = 1
        taskCtMap = self.fe_l_rot_cur_W @ taskCtMap @ self.fe_l_rot_cur_W.T
        self.kin_tasks_stand.task_list[id].J = self.Jfe.copy()
        self.kin_tasks_stand.task_list[id].J[3: 6, : self.model_nv] = taskCtMap @ self.kin_tasks_stand.task_list[id].J[
                                                                                  3: 6,
                                                                                  : self.model_nv]
        self.kin_tasks_stand.task_list[id].J[9: 12, : self.model_nv] = taskCtMap @ self.kin_tasks_stand.task_list[id].J[
                                                                                   9: 12,
                                                                                   : self.model_nv]
        self.kin_tasks_stand.task_list[id].J[0: 12, 22: 25] = 0
        self.kin_tasks_stand.task_list[id].dJ = np.zeros((12, self.model_nv))
        self.kin_tasks_stand.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_stand.getId("HipRPY")
        self.kin_tasks_stand.task_list[id].errX = np.zeros(3)
        desRot = Transformation.eul2Rot(0, 0, 0)
        self.kin_tasks_stand.task_list[id].errX[0: 3] = Transformation.diffRot(self.hip_link_rot, desRot)
        self.kin_tasks_stand.task_list[id].derrX = np.zeros(3)
        self.kin_tasks_stand.task_list[id].ddxDes = np.zeros(3)
        self.kin_tasks_stand.task_list[id].dxDes = np.zeros(3)
        self.kin_tasks_stand.task_list[id].kp = np.eye(3) * 1000
        self.kin_tasks_stand.task_list[id].kd = np.eye(3) * 50
        taskMapRPY = np.zeros((3, 6))
        taskMapRPY[0, 3] = 1
        taskMapRPY[1, 4] = 1
        taskMapRPY[2, 5] = 1
        self.kin_tasks_stand.task_list[id].J = taskMapRPY @ self.J_hip_link
        self.kin_tasks_stand.task_list[id].dJ = np.zeros((3, self.model_nv))
        self.kin_tasks_stand.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_stand.getId("Pz")
        self.kin_tasks_stand.task_list[id].errX = np.zeros(1)
        self.kin_tasks_stand.task_list[id].errX[0] = self.base_pos_des[2] - self.q[2]
        self.kin_tasks_stand.task_list[id].derrX = np.zeros(1)
        self.kin_tasks_stand.task_list[id].ddxDes = np.zeros(1)
        self.kin_tasks_stand.task_list[id].dxDes = np.zeros(1)
        self.kin_tasks_stand.task_list[id].kp = np.eye(1) * 2000
        self.kin_tasks_stand.task_list[id].kd = np.eye(1) * 10
        taskMap = np.zeros((1, 6))
        taskMap[0, 2] = 1
        self.kin_tasks_stand.task_list[id].J = taskMap @ self.J_base
        self.kin_tasks_stand.task_list[id].dJ = taskMap @ self.dJ_base
        self.kin_tasks_stand.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_stand.getId("CoMTrack")
        self.kin_tasks_stand.task_list[id].errX = np.zeros(2)
        self.kin_tasks_stand.task_list[id].errX = self.pCoMDes[0: 2] - self.pCoMCur[0: 2]
        self.kin_tasks_stand.task_list[id].derrX = np.zeros(2)
        self.kin_tasks_stand.task_list[id].ddxDes = np.zeros(2)
        self.kin_tasks_stand.task_list[id].dxDes = np.zeros(2)
        self.kin_tasks_stand.task_list[id].kp = np.eye(2) * 2000
        self.kin_tasks_stand.task_list[id].kd = np.eye(2) * 100
        self.kin_tasks_stand.task_list[id].J = self.Jcom[0: 2, 0: self.model_nv]
        self.kin_tasks_stand.task_list[id].dJ = np.zeros((2, self.model_nv))
        self.kin_tasks_stand.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_stand.getId("CoMXY_HipRPY")
        taskMapRPY = np.zeros((3, 6))
        taskMapRPY[0, 3] = 1
        taskMapRPY[1, 4] = 1
        taskMapRPY[2, 5] = 1
        self.kin_tasks_stand.task_list[id].errX = np.zeros(5)
        self.kin_tasks_stand.task_list[id].errX[0: 2] = self.pCoMDes[0: 2] - self.pCoMCur[0: 2]
        desRot = Transformation.eul2Rot(self.base_rpy_des[0], self.base_rpy_des[1], self.base_rpy_des[2])
        self.kin_tasks_stand.task_list[id].errX[2: 5] = Transformation.diffRot(self.hip_link_rot, desRot)
        self.kin_tasks_stand.task_list[id].derrX = np.zeros(5)
        self.kin_tasks_stand.task_list[id].ddxDes = np.zeros(5)
        self.kin_tasks_stand.task_list[id].dxDes = np.zeros(5)
        self.kin_tasks_stand.task_list[id].kp = np.eye(5) * 100
        self.kin_tasks_stand.task_list[id].kd = np.eye(5) * 10
        self.kin_tasks_stand.task_list[id].kp[2: 5, 2: 5] = np.eye(3) * 100
        self.kin_tasks_stand.task_list[id].kd[2: 5, 2: 5] = np.eye(3) * 10
        self.kin_tasks_stand.task_list[id].J = np.zeros((5, self.model_nv))
        self.kin_tasks_stand.task_list[id].J[0: 2, 0: self.model_nv] = self.Jcom[0: 2, 0: self.model_nv]
        self.kin_tasks_stand.task_list[id].J[2: 5, 0: self.model_nv] = taskMapRPY @ self.J_hip_link
        self.kin_tasks_stand.task_list[id].dJ = np.zeros((5, self.model_nv))
        self.kin_tasks_stand.task_list[id].W = np.eye(self.model_nv)

        id = self.kin_tasks_stand.getId("Roll_Pitch_Yaw")
        self.kin_tasks_stand.task_list[id].errX = np.zeros(3)
        desRot = Transformation.eul2Rot(self.base_rpy_des[0], self.base_rpy_des[1], self.base_rpy_des[2])
        self.kin_tasks_stand.task_list[id].errX = Transformation.diffRot(self.base_rot, desRot)
        self.kin_tasks_stand.task_list[id].derrX = np.zeros(3)
        self.kin_tasks_stand.task_list[id].derrX = -self.dq[3: 6]
        self.kin_tasks_stand.task_list[id].ddxDes = np.zeros(3)
        self.kin_tasks_stand.task_list[id].dxDes = np.zeros(3)
        self.kin_tasks_stand.task_list[id].kp = np.eye(3) * 2000
        self.kin_tasks_stand.task_list[id].kd = np.eye(3) * 100
        taskMap = np.zeros((3, 6))
        taskMap[0, 3] = 1
        taskMap[1, 4] = 1
        taskMap[2, 5] = 1
        self.kin_tasks_stand.task_list[id].J = taskMap @ self.J_base
        self.kin_tasks_stand.task_list[id].dJ = taskMap @ self.dJ_base
        self.kin_tasks_stand.task_list[id].W = np.eye(self.model_nv)

        if self.motionStateCur == MotionState.WALK or self.motionStateCur == MotionState.WALK2STAND:
            self.kin_tasks_walk.calculate(self.des_delta_q, self.des_dq, self.des_ddq, self.dyn_M, self.dyn_M_inv,
                                          self.dq)
            self.delta_q_final_kin[:] = self.kin_tasks_walk.out_delta_q
            self.dq_final_kin[:] = self.kin_tasks_walk.out_dq
            self.ddq_final_kin[:] = self.kin_tasks_walk.out_ddq
        elif self.motionStateCur == MotionState.STAND:
            self.kin_tasks_stand.calculate(self.des_delta_q, self.des_dq, self.des_ddq, self.dyn_M, self.dyn_M_inv,
                                           self.dq)
            self.delta_q_final_kin[:] = self.kin_tasks_stand.out_delta_q
            self.dq_final_kin[:] = self.kin_tasks_stand.out_dq
            self.ddq_final_kin[:] = self.kin_tasks_stand.out_ddq
        else:
            self.delta_q_final_kin[:] = np.zeros(self.model_nv)
            self.dq_final_kin[:] = np.zeros(self.model_nv)
            self.ddq_final_kin[:] = np.zeros(self.model_nv)
